/*
 * ControlLoop.cpp
 *
 *  Created on: Aug 6, 2009
 *      Author: naveenoid
 */
#define STRING_TEST 2

#include "ControlLoop.hpp"
#include <ecl/time.hpp>
#include <stdio.h>
#include "./../Common/Common.hpp"

using Octopus::Common::sWait;
using Octopus::Common::uWait;

using Octopus::BoardSetup::TitechBoard;
using ECL::Time::StopWatch;
using ECL::Time::TimeStamp;
using Octopus::ToolKit::Log::Logger;
//using ECL::Time::

namespace Octopus {

namespace MainLoop {

//ControlLoop * ControlLoop::controlLoop;


Loop * ControlLoop::getInstance(Logger *log) {
	if(loop == NULL)
	{
		loop = new ControlLoop(log);
	}
	return(loop);
}

void ControlLoop::remove() {
	printf("Removing Control Loop\n");

	while(!loopFinished)
	{
//		uWait(10000);
		sWait(0.1);
		printf("waiting for thread to finish\n");
	}

	delete(this);
}

void ControlLoop::start() {

	int ctr = 1;
	StopWatch stopwatch;
	TimeStamp time;

	long int loopTime =  (long int)LOOP_TIME;
	long int time1 = 0, time2 = 0, time3 = 0, timeDiff = 0, totTime = 0, time1Prev = 0;
	double avgTime;

	int lMeasDigital=0, motorOutDigital= 0;
	int pulseOut = 0;
	double fMeasAnalog = 0.0, motorOutAnalog = 0.0;
	//, TsetPoint = 0.0, lastError = 0.0, errorDot = 0.0;

	vector<double>TsetPoint(MAX_NUM_OF_MOTOR, 0.0);
	vector<double>lastError(MAX_NUM_OF_MOTOR, 0.0);
	vector<double>errorDot(MAX_NUM_OF_MOTOR, 0.0);
	vector<double>errorInt(MAX_NUM_OF_MOTOR, 0.0);
	vector<double>offset(MAX_NUM_OF_MOTOR, 0.0);
	vector<double>error(MAX_NUM_OF_MOTOR,0.0);
	vector<bool> isMotorReady(MAX_NUM_OF_MOTOR,false);

	//double errorInt = 0.0;
	//double offset = 0.0;

	bool initialising = true;

	loopFinished = false;
	//Initialising Loop

	printf("Starting Pretension Initialisation\n\n");
	while(initialising)
	{
		mutex.lock();
		if(!continueLoop)
		{
			printf("Loop trying to stop\n");
			mutex.unlock();
			break;
		}
		mutex.unlock();

		time = stopwatch.elapsed();
		time1 = time.usec();
//		printf("Initial = %ld \t",time1);
//		wait(0.10);

//		uWait(50000);
// Task

//		vector<int> temp(MAX_NUM_OF_SENSOR,0);
		vector<int> temp = titechBoard->readAD();
//		printf("getting each sensor from vector  ");
		bool tempFlag = true;
		for(int i=0; i<MAX_NUM_OF_MOTOR && (unsigned int)i<temp.size();i++)
//		for(int i=0; i<temp.size();i++)
		{
			if(isMotorReady.at(i)) {
//				initialising = true;
				continue;
			}
			else {
				lMeasDigital = temp.at(i);
				//			fMeasAnalog = POT_SPRING_STIFFNESS * POT_MAX_DISTANCE * ((1023 - (double)lMeasDigital)/1023);
				fMeasAnalog = POT_SPRING_STIFFNESS * POT_MAX_DISTANCE * (((double)lMeasDigital)/1023);

				pulseOut = (int)(1100+ 800*((double)offset.at(i)/1260));
//			double tempPulseOut = (((double)pulseOut - 1100)*((3.5*360)/800));

				printf("%d: %f/%f; ", i+1,fMeasAnalog, offset.at(i));
				titechBoard->writeMotor(i,pulseOut);

				if(fMeasAnalog>0.0)	{
//					initialising = false;
					isMotorReady.at(i) = true;
				}
				else {
//					initialising = true;
					isMotorReady.at(i) = false;

					if(offset.at(i)<100.0) {
						offset.at(i)+=50.0;
					}
					else if(offset.at(i) >= 100.0 && offset.at(i) <= 130.0)	{
						offset.at(i)+=10.0;
					}
					else if(offset.at(i)>130.0)	{
						offset.at(i)+=0.1;
					}
					//			printf("Sensor %d : %d ", i+1, temp.at(i+1));
				}
				tempFlag = tempFlag && isMotorReady.at(i);
			}

			initialising = !tempFlag;

		}
		printf("\n");

		time = stopwatch.elapsed();
		time2 = time.usec();
//		printf("Final = %ld \t",time2);

		timeDiff = loopTime-(time2-time1);
//		printf("Must wait for %ld uSec\n", timeDiff);

		uWait(timeDiff);
		stopwatch.restart();

	}


//	loopTime = (long int)LOOP_TIME;

//	for(int i=0; i<MAX_NUM_OF_MOTOR;i++)
//	{
//		offset.at(i) = 0.75 * offset.at(i);
//	}

	printf("\n\nFinished Pretension Initialisation, Starting Settling\n\n");

	vector<double> tempOffset(MAX_NUM_OF_MOTOR,0.0);
	for(int i=0;i<MAX_NUM_OF_MOTOR;i++)	{
		TsetPoint.at(i) = 0.1;
		offset.at(i) = 0.90 * offset.at(i);

	}
	bool finishedSettling = false;
	int loopCtr = 0;

	while(!finishedSettling)
	{
		mutex.lock();
		if(!continueLoop)
		{
			printf("Loop trying to stop\n");
			mutex.unlock();
			break;
		}
		mutex.unlock();

		time = stopwatch.elapsed();
		time1 = time.usec();
//		printf("Initial = %ld \t",time1);
//		wait(0.10);

//		uWait(50000);
// Task

//		vector<int> temp(MAX_NUM_OF_SENSOR,0);
		vector<int> temp = titechBoard->readAD();
//		printf("getting each sensor from vector  ");

//		setPointMutex.lock();
//		TsetPoint = setPoints;
//		setPointMutex.unlock();

		printf("%d : ", loopCtr++);
		for(int i=0; i<MAX_NUM_OF_MOTOR && (unsigned int)i<temp.size();i++)
//		for(int i=0; i<temp.size();i++)
		{
			lMeasDigital = temp.at(i);
//			fMeasAnalog = POT_SPRING_STIFFNESS * POT_MAX_DISTANCE * ((1023 - (double)lMeasDigital)/1023);
			fMeasAnalog = POT_SPRING_STIFFNESS * POT_MAX_DISTANCE * (((double)lMeasDigital)/1023);

			lastError.at(i) = error.at(i);
			error.at(i) = TsetPoint.at(i) - fMeasAnalog;
			errorInt.at(i) += (error.at(i) * loopTime) / 1000000;
			errorDot.at(i) = 1000000 * ((error.at(i)-lastError.at(i)) / loopTime);

			motorOutAnalog = (TsetPoint.at(i) * 360)/(PI * WHEEL_DIAMETER* (POT_SPRING_STIFFNESS + LOAD_SPRING_STIFFNESS)) + (double)K_P * error.at(i) + (double)K_D * errorDot.at(i) + (double)K_I * errorInt.at(i);

			motorOutDigital = 1100 + (int)((((double)motorOutAnalog + offset.at(i))*800)/(3.5*360));
			if(motorOutDigital<1100) {

				motorOutDigital = 1100;
			}

			tempOffset.at(i) = motorOutAnalog+offset.at(i);

//			pulseOut = (int)(1100 + 800*((double)lMeasDigital/1023));

//			double thetaTemp = ((double)theta/1023);
//			pulseOut = (int)(1100 + 800*((double)heta/1023));
//			pulseOut = (int)(1100+ 800*(offset/1260));


//			double tempPulseOut = (((double)pulseOut - 1100)*((3.5*360)/800));

//			printf("%d: SP: %f, Act(N): %f, e: %f, eDot: %f, eInt: %f, KPe : %f, KDeDot : %f, KIeInt : %f", i, TsetPoint.at(i), fMeasAnalog, error.at(i), errorDot.at(i), errorInt.at(i), (double)K_P * error.at(i), (double)K_D * errorDot.at(i), (double)K_I * errorInt.at(i));
			printf("%d: Off: %f, Act: %f; ", i+1,tempOffset.at(i), error.at(i));
//			titechBoard->writeMotor(i,pulseOut);
			titechBoard->writeMotor(i,motorOutDigital);

//			printf("Sensor %d : %d ", i+1, temp.at(i+1));
		}
		printf("\n");

		if(( (error.at(0) < 0.1) && (error.at(1) < 0.1) && (error.at(2) < 0.1) && (error.at(3) < 0.1)) || loopCtr>100)
		{
			finishedSettling = true;
		}
		time = stopwatch.elapsed();
		time2 = time.usec();
//		printf("Final = %ld \t",time2);

		timeDiff = loopTime-(time2-time1);
//		printf("Must wait for %ld uSec\n", timeDiff);

		uWait(timeDiff);

		time = stopwatch.elapsed();

		time3 = time.usec();

		// Job
		totTime += time3 - time1;
		avgTime = (double)totTime / (double)ctr;
		ctr++;

//		printf ("Time3 is %ld uSec and total loopTime is %ld uSec and Average LoopTime is %f\n", time3, totTime, avgTime);

		time1Prev = time1;

		stopwatch.restart();

	}

	offset = tempOffset;

	printf("Finished Settling, Starting Control/Monitoring\n");




#if LOOP_TYPE == 1
	while(true)
	{
		mutex.lock();
		if(!continueLoop)
		{
			printf("Loop trying to stop\n");
			mutex.unlock();
			break;
		}
		mutex.unlock();

		time = stopwatch.elapsed();
		time1 = time.usec();
//		printf("Initial = %ld \t",time1);
//		wait(0.10);

//		uWait(50000);
// Task

//		vector<int> temp(MAX_NUM_OF_SENSOR,0);
		vector<int> temp = titechBoard->readAD();
//		printf("getting each sensor from vector  ");

		setPointMutex.lock();
		TsetPoint = setPoints;
		setPointMutex.unlock();

		for(int i=0; i<MAX_NUM_OF_MOTOR && (unsigned int)i<temp.size();i++)
//		for(int i=0; i<temp.size();i++)
		{
			lMeasDigital = temp.at(i);
//			fMeasAnalog = POT_SPRING_STIFFNESS * POT_MAX_DISTANCE * ((1023 - (double)lMeasDigital)/1023);
			fMeasAnalog = POT_SPRING_STIFFNESS * POT_MAX_DISTANCE * (((double)lMeasDigital)/1023);

//			lastError.at(i) = error.at(i);
//			error.at(i) = TsetPoint.at(i) - fMeasAnalog;
//			errorInt.at(i) += (error.at(i) * loopTime) / 1000000;
//			errorDot.at(i) = 1000000 * ((error.at(i)-lastError.at(i)) / loopTime);
//
//			motorOutAnalog = (TsetPoint.at(i) * 360)/(PI * WHEEL_DIAMETER* (POT_SPRING_STIFFNESS + LOAD_SPRING_STIFFNESS)) + (double)K_P * error.at(i) + (double)K_D * errorDot.at(i) + (double)K_I * errorInt.at(i);
//
//			motorOutDigital = 1100 + (int)((((double)motorOutAnalog + offset.at(i))*800)/(3.5*360));
//			if(motorOutDigital<1100) {
//
//				motorOutDigital = 1100;
//			}

//			pulseOut = (int)(1100 + 800*((double)lMeasDigital/1023));

//			double thetaTemp = ((double)theta/1023);
//			pulseOut = (int)(1100 + 800*((double)heta/1023));
//			pulseOut = (int)(1100+ 800*(offset/1260));


//			double tempPulseOut = (((double)pulseOut - 1100)*((3.5*360)/800));

			printf("%d : Act(N): %f", i+1, fMeasAnalog);
//			titechBoard->writeMotor(i,pulseOut);
//			titechBoard->writeMotor(i,motorOutDigital);

//			printf("Sensor %d : %d ", i+1, temp.at(i+1));
		}
		printf("\n");


		time = stopwatch.elapsed();
		time2 = time.usec();
//		printf("Final = %ld \t",time2);

		timeDiff = loopTime-(time2-time1);
//		printf("Must wait for %ld uSec\n", timeDiff);

		uWait(timeDiff);

		time = stopwatch.elapsed();

		time3 = time.usec();

		// Job
		totTime += time3 - time1;
		avgTime = (double)totTime / (double)ctr;
		ctr++;

//		printf ("Time3 is %ld uSec and total loopTime is %ld uSec and Average LoopTime is %f\n", time3, totTime, avgTime);

		time1Prev = time1;

		stopwatch.restart();

	}

#else
	//Main LOOP
	while(true)
	{
		mutex.lock();
		if(!continueLoop)
		{
			printf("Loop trying to stop\n");
			mutex.unlock();
			break;
		}
		mutex.unlock();

		time = stopwatch.elapsed();
		time1 = time.usec();
//		printf("Initial = %ld \t",time1);
//		wait(0.10);

//		uWait(50000);
// Task

//		vector<int> temp(MAX_NUM_OF_SENSOR,0);
		vector<int> temp = titechBoard->readAD();
//		printf("getting each sensor from vector  ");

		setPointMutex.lock();
		TsetPoint = setPoints;
		setPointMutex.unlock();

		for(int i=0; i<MAX_NUM_OF_MOTOR && (unsigned int)i<temp.size();i++)
//		for(int i=0; i<temp.size();i++)
		{
			lMeasDigital = temp.at(i);
//			fMeasAnalog = POT_SPRING_STIFFNESS * POT_MAX_DISTANCE * ((1023 - (double)lMeasDigital)/1023);
			fMeasAnalog = POT_SPRING_STIFFNESS * POT_MAX_DISTANCE * (((double)lMeasDigital)/1023);

			lastError.at(i) = error.at(i);
			error.at(i) = TsetPoint.at(i) - fMeasAnalog;
			errorInt.at(i) += (error.at(i) * loopTime) / 1000000;
			errorDot.at(i) = 1000000 * ((error.at(i)-lastError.at(i)) / loopTime);

			motorOutAnalog = (TsetPoint.at(i) * 360)/(PI * WHEEL_DIAMETER* (POT_SPRING_STIFFNESS + LOAD_SPRING_STIFFNESS)) + (double)K_P * error.at(i) + (double)K_D * errorDot.at(i) + (double)K_I * errorInt.at(i);

			motorOutDigital = 1100 + (int)((((double)motorOutAnalog + offset.at(i))*800)/(3.5*360));
			if(motorOutDigital<1100) {

				motorOutDigital = 1100;
			}

//			pulseOut = (int)(1100 + 800*((double)lMeasDigital/1023));

//			double thetaTemp = ((double)theta/1023);
//			pulseOut = (int)(1100 + 800*((double)heta/1023));
//			pulseOut = (int)(1100+ 800*(offset/1260));


//			double tempPulseOut = (((double)pulseOut - 1100)*((3.5*360)/800));

//			printf("%d: SP: %f, Act(N): %f, e: %f, eDot: %f, eInt: %f, KPe : %f, KDeDot : %f, KIeInt : %f", i, TsetPoint.at(i), fMeasAnalog, error.at(i), errorDot.at(i), errorInt.at(i), (double)K_P * error.at(i), (double)K_D * errorDot.at(i), (double)K_I * errorInt.at(i));
			printf("%d: SP: %f, Act(N): %f, e: %f", i, TsetPoint.at(i), fMeasAnalog,error.at(i));
//			titechBoard->writeMotor(i,pulseOut);
			titechBoard->writeMotor(i,motorOutDigital);

//			printf("Sensor %d : %d ", i+1, temp.at(i+1));
		}
		printf("\n");


		time = stopwatch.elapsed();
		time2 = time.usec();
//		printf("Final = %ld \t",time2);

		timeDiff = loopTime-(time2-time1);
//		printf("Must wait for %ld uSec\n", timeDiff);

		uWait(timeDiff);

		time = stopwatch.elapsed();

		time3 = time.usec();

		// Job
		totTime += time3 - time1;
		avgTime = (double)totTime / (double)ctr;
		ctr++;

//		printf ("Time3 is %ld uSec and total loopTime is %ld uSec and Average LoopTime is %f\n", time3, totTime, avgTime);

		time1Prev = time1;

		stopwatch.restart();

	}
#endif

	//Resetting motors

	for(int i=0; i<MAX_NUM_OF_MOTOR ;i++)
	//		for(int i=0; i<temp.size();i++)
	{

		titechBoard->writeMotor(i,1100);
	}


	loopFinished = true;
	printf ("Average LoopTime was %f\n", avgTime);
	printf("ControlLoop thread closing\n");
}

bool ControlLoop::stop()
{
	printf("Trying to stop Loop");
	mutex.lock();
	continueLoop = false;
	mutex.unlock();
	return(continueLoop);
}
bool ControlLoop::finished()
{
	return(loopFinished);
}

bool ControlLoop::changeSetPoints(vector<double> sp) {
	setPointMutex.lock();
	setPoints = sp;
	setPointMutex.unlock();
	return(true);
}

ControlLoop::ControlLoop(Logger *log) : sensorValues(MAX_NUM_OF_SENSOR,0), motorCommands(MAX_NUM_OF_MOTOR,0), setPoints(MAX_NUM_OF_MOTOR,0.0) {
	logger = log;
	titechBoard = TitechBoard::getInstance();
	continueLoop = true;
	loopFinished = true;
	offsetForce = 0.0;
	printf("Created New ControlLoop\n");
}

ControlLoop::~ControlLoop() {
	titechBoard->remove();
}

}

}
